//  ****  Serving A Drink Robotic Arm   ****
    //  Automated Robotic Arm using 6 servos (6 DOF).
    //  Using HC-SR04 distance sensor module.
    //  Written by G. Rosa   January 17, 2023.
      
    #include <Servo.h>           //include servo library
    #include "NewPing.h"
    
    // Hook up HC-SR04 with Trig to Arduino Pin 10, Echo to Arduino pin 13
    // Maximum Distance is 400 cm
     
    #define TRIGGER_PIN  11    // Note: Pin for Distance Sensor
    #define ECHO_PIN     13    // Note: Pin for Distance Sensor
    #define MAX_DISTANCE 400
     
    NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

    int pos = 0; // variable to store the servo position
    float distance;

    Servo myservo1;
    Servo myservo2;
    Servo myservo3;
    Servo myservo4;
    Servo myservo5; 
    Servo myservo6;
    
    int myservo1_pos= 82;     //  ** starting position for base
    int myservo2_pos= 180;    //  ** starting position for shoulder
    int myservo3_pos= 180;    //  ** starting position for elbow
    int myservo4_pos= 140;    //  ** starting position for wrist-bend
    int myservo5_pos= 180;    //  ** starting position for wrist-twist
    int myservo6_pos= 180;    //  ** starting position for claw

    void setup ( ) 
    {
    Serial.begin (9600) ;
    
    myservo6.attach(5);     // claw
    myservo5.attach(6);     // wrist-twist
    myservo4.attach(7);     // wrist-bend
    myservo1.attach(8);     // base    
    myservo2.attach(9);     // shoulder
    myservo3.attach(10);    // elbow
       
    myservo1.write(myservo1_pos);   
    myservo2.write(myservo2_pos);   
    myservo3.write(myservo3_pos);  
    myservo4.write(myservo4_pos);  
    myservo5.write(myservo5_pos);  
    myservo6.write(myservo6_pos);
   }

  void loop ( ) 
   {

  distance = sonar.ping_cm();
  
  // Send results to Serial Monitor
  
  if (distance == 20.00 || distance == 21 ) {
    Serial.print("Distance = ");
    Serial.print(distance);
    Serial.println(" cm");

    myservo6_forward_180_degrees();       // claw
     
    Forward_Step1();       
        
    Forward_Step2();       

    //---------------------------------------------------------------------------
          
    myservo6_backwards_180_degrees();     // claw
           
    myservo2_and_4_backwards_120_degrees();   // wrist-bendB + shoulderB1

    myservo2_backwards_180_degrees2();  

   //---------------------------------------------------------------------------

    myservo1_forward_82_degrees();        // baseF
            
    myservo2_forward_180_degrees3();      // shoulderF3

    myservo5_forward_180_degrees();        // wrist twist left
    
    delay(80);

    myservo5_forward_180_degrees2();        // wrist twist left

    delay(120);

    myservo5_backwards_180_degrees();      // wrist twist right    

    myservo2_backwards_180_degrees3();    // shoulderB2 + elbowB2

    myservo1_backwards_82_degrees();      // baseB

    Forward_Step1();       
        
    Forward_Step2();

    myservo6_forward_180_degrees();       // clawF    
               
    myservo2_and_4_backwards_120_degrees();   // wrist-bendB + shoulderB1

    myservo2_backwards_180_degrees2();  

    myservo6_backwards_180_degrees();     // clawB

    delay(160);
            
    }
  else {
    Serial.print(distance);
    Serial.println(" cm");
    Serial.println("Out of range");
    delay(200);
  }
  
  delay(200);
 } // End of Loop

// *****  FUNCTION ROUTINES  ***********

void myservo6_forward_180_degrees() {         //clawF
      for (pos = 180; pos >= 0; pos -= 1)   
          { myservo6.write(pos);
          Serial.println(pos);
          delay(5);
             }             
      }

// Step 1
void Forward_Step1() {      //shoulderB2 + elbowB2
      for (pos = 180; pos >= 140; pos -= 1) 
         { myservo2.write(pos);
           myservo3.write(pos);
           Serial.println(pos);
           delay(20);
       }  
       for (pos = 140; pos <= 150; pos +=1)
         { myservo3.write(pos);  
           Serial.println(pos);
           delay(20);  
        }
  }

// Step 2
 void Forward_Step2() {      //wrist-bendB + shoulderB1
       for (pos = 140; pos >= 40; pos -= 1) 
         { myservo2.write(pos);
           myservo4.write(pos);
           Serial.println(pos);
           delay(20);
       }  
           myservo2.write(44);
       for (pos = 40; pos >= 10; pos -= 1) 
         { myservo4.write(pos);
           Serial.println(pos);
           delay(20);
       } 
           
  }

// Claw
void myservo6_backwards_180_degrees() {      //clawB 
       for (pos = 0; pos <= 180; pos += 1) 
           { myservo6.write(pos);
           Serial.println(pos);
           delay(5);
             }  
       }

// Return shoulder and elbow back to original 180 degree position.
// servo 2 - shoulder   servo 3 - elbow    servo 4  - wrist-bend

  void myservo2_and_4_backwards_120_degrees() {      //wrist-bendB + shoulderB1
     for (pos = 40; pos <= 120; pos += 1) 
         { myservo2.write(pos);
           myservo4.write(pos + 20);
           Serial.println(pos);
           delay(20);
       }  
  }

 void myservo2_backwards_180_degrees2() {      //shoulderB2 + elbowB2
     for (pos = 120; pos <= 180; pos += 1) 
         { myservo2.write(pos);
              if (pos >= 150 ) {
              myservo3.write(pos);
              Serial.println(pos);
              delay(20);
                }
           Serial.println(pos);
           delay(20);
        }  
  }

// --------------------------------------------------------

void myservo1_backwards_82_degrees() {       //baseB
     for (pos = 148; pos >= 82; pos -= 1) 
        { myservo1.write(pos);
          Serial.println(pos);
          delay(20);
       }
  }

 // --------------------------------------------------------------------

  void myservo1_forward_82_degrees()  {         // baseF
     for (pos = 82; pos <= 148; pos += 1) 
       {  myservo1.write(pos);
          //Serial.println(pos);
          delay(20);
       }
  }

 void myservo2_forward_180_degrees3() {        //shoulderF3
      for (pos = 180; pos >= 160; pos -= 1) 
        { myservo2.write(pos);
          Serial.println(pos);
          delay(20);
       }
  }

void myservo2_backwards_180_degrees3() {      //shoulderB2 + elbowB2
     for (pos = 160; pos <= 180; pos += 1) 
         { myservo2.write(pos);
              if (pos >= 150 ) {
              myservo3.write(pos);
              Serial.println(pos);
              delay(20);
                }
           Serial.println(pos);
           delay(20);
        }  
  }

// --------------------------------------------------------

  void myservo5_forward_180_degrees() {       //wrist-twist left
      for (pos = 180; pos >= 90; pos -= 1)   
          { myservo5.write(pos);
          Serial.println(pos);
          delay(50);
       }
  }

  void myservo5_forward_180_degrees2() {       //wrist-twist left
      for (pos = 90; pos >= 60; pos -= 1)   
          { myservo5.write(pos);
          Serial.println(pos);
          delay(60);
       }
  }

 void myservo5_backwards_180_degrees() {      //wrist-twist right
       for (pos = 60; pos <= 180; pos += 1) 
           { myservo5.write(pos);
           Serial.println(pos);
           delay(40);
       }  
  }
 
//  * ===================== End of Program  ========================== *